#include "grbl.h"


#ifdef VARIABLE_SPINDLE
  static float pwm_gradient; // Precalulated value to speed up rpm to PWM conversions.
#endif

//#define DEBUG_SERVO
#define PEN_SERVO_180_TICKS     31
#define PEN_SERVO_0_TICKS       16
#define PEN_DOWN_STEPS       20
#define PEN_DOWN_SLOW_FACTOR 0.1  //between 0 and 1. 0 mean no pen_down, 1 meas full speed.


#define SERVO_PWM_DDR	  DDRB
#define SPINDLE_PWM_PORT  PORTB
#define SERVO_PWM_BIT	  3    // Uno Digital Pin 11 
#define SERVO_TCCRA_REGISTER	  TCCR2A
#define SERVO_TCCRB_REGISTER	  TCCR2B
#define SERVO_OCR_REGISTER      OCR2A
#define SERVO_COMB_BIT	        COM2A1


#ifdef DEBUG_SERVO
void debug_digit_info(const char* info, uint8_t info_size,int number,int endWithNewLine)
{
	for(int i=0;i<info_size;i++)
		serial_write(info[i]);
	print_uint8_base10(number); 
	if(endWithNewLine)
		serial_write('\n');
}
#endif	

//请参考H:\3DPrinter\Plotter\Using Grbl’s Spindle PWM to Control a Servo at Buildlog.Net Blog.pdf
/*
M3 S90    // 应该转到90度位置
M3 S180   // 应该转到180度位置  
M3 S0     // 应该转到0度位置
M5        // 停止，回到0度
*/
void init_servo()
{
	SERVO_PWM_DDR |= (1<<SERVO_PWM_BIT); // Configure as output pin.
	
	SERVO_TCCRA_REGISTER=0;
	SERVO_TCCRA_REGISTER = (1<<COM2A1) | ((1<<WGM20) | (1<<WGM21));
	
	SERVO_TCCRB_REGISTER=0;
	SERVO_TCCRB_REGISTER = (1<<CS22) | (1 <<CS21) | (1<<CS20);
	SERVO_OCR_REGISTER=PEN_UP_POS;    
}

uint8_t get_servo_pwm_value(float angle)
{
	// 使用$30和$31参数进行线性映射
	float ratio;

	//参数配置错误，使用默认映射，将转速限制在0-180范围内
	if (angle < 0) angle = 0;
	if (angle > 180) angle = 180;
	ratio = angle / 180.0;

   // 映射到舵机PWM范围（根据您的硬件调整）
   // 计算脉冲宽度：1ms ~ 2ms 对应 0~180度
   // 1ms = 16MHz/1024/1000 = 15.625counts=16counts
   // 2ms = 2*16MHz/1024/1000 = 15.625*2=31.25counts=31counts	   
   uint8_t pwm_value = PEN_SERVO_0_TICKS + ratio * (PEN_SERVO_180_TICKS - PEN_SERVO_0_TICKS);

	#ifdef DEBUG_SERVO
		char prompt[]="input angle";
		debug_digit_info(prompt,sizeof(prompt),(int)angle,0);
		char info[]="set servo pos:";
		debug_digit_info(info,sizeof(info),pwm_value,1);			
	#endif
	return pwm_value;
}

void spindle_init()
{
  #ifdef VARIABLE_SPINDLE
	init_servo();
  #endif
  spindle_stop();
}

void set_pen_pos(float angle)
{
  SERVO_OCR_REGISTER=get_servo_pwm_value(angle);
}

uint8_t spindle_get_state()
{
  #ifdef VARIABLE_SPINDLE
    #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
      // No spindle direction output pin. 
      #ifdef INVERT_SPINDLE_ENABLE_PIN
        if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
      #else
        if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { return(SPINDLE_STATE_CW); }
      #endif
    #else
      if (SPINDLE_TCCRA_REGISTER & (1<<SPINDLE_COMB_BIT)) { // Check if PWM is enabled.
        #ifdef ENABLE_DUAL_AXIS
          return(SPINDLE_STATE_CW);
        #else
          if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
          else { return(SPINDLE_STATE_CW); }
        #endif
      }
    #endif
  #else
    #ifdef INVERT_SPINDLE_ENABLE_PIN
      if (bit_isfalse(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) { 
    #else
      if (bit_istrue(SPINDLE_ENABLE_PORT,(1<<SPINDLE_ENABLE_BIT))) {
    #endif
      #ifdef ENABLE_DUAL_AXIS    
        return(SPINDLE_STATE_CW);
      #else
        if (SPINDLE_DIRECTION_PORT & (1<<SPINDLE_DIRECTION_BIT)) { return(SPINDLE_STATE_CCW); }
        else { return(SPINDLE_STATE_CW); }
      #endif
    }
  #endif
  return(SPINDLE_STATE_DISABLE);
}


// Disables the spindle and sets PWM output to zero when PWM variable spindle speed is enabled.
// Called by various main program and ISR routines. Keep routine small, fast, and efficient.
// Called by spindle_init(), spindle_set_speed(), spindle_set_state(), and mc_reset().
void spindle_stop()
{
  set_pen_pos(PEN_UP_POS);	
}


#ifdef VARIABLE_SPINDLE
	// Sets spindle speed PWM output and enable pin, if configured. Called by spindle_set_state()
	// and stepper ISR. Keep routine small and efficient.
	void spindle_set_speed(uint8_t pwm_value)
	{
    #ifdef DEBUG_SERVO
      char prompt[]="Invoking spindle_set_speed with pwm_value:";
      debug_digit_info(prompt,sizeof(prompt),pwm_value,1);
	  #endif

		if (pwm_value == SPINDLE_PWM_OFF_VALUE) {
		  spindle_stop();
		} 
    else if(SERVO_OCR_REGISTER==pwm_value)
      return;
    else
    {
      if(pwm_value==get_servo_pwm_value(PEN_DOWN_POS))
      {
        //slowly down the pen to PEN_DOWN_POS
        int i=0;
        float pwm_start=SERVO_OCR_REGISTER;
        float gradual_pwm=pwm_start+(pwm_value-pwm_start)*0.2;
        float pwm_delta=((float)pwm_value-gradual_pwm)/PEN_DOWN_STEPS;
        for(i=0;i<PEN_DOWN_STEPS;i++,gradual_pwm+=pwm_delta)
        {
          #ifdef DEBUG_SERVO
            char prompt[]="DOWN counter";
            debug_digit_info(prompt,sizeof(prompt),i,0);
            char info[]="DOWN pos:";
            debug_digit_info(info,sizeof(info),gradual_pwm,1);
	        #endif
          SERVO_OCR_REGISTER = (uint8_t)gradual_pwm; // Set PWM output level.  
          if(SERVO_OCR_REGISTER==pwm_value)          
            break;

          if(gradual_pwm==pwm_start)
            continue;
          
          delay_ms(30);
          pwm_start=gradual_pwm;
        }

        // int i=0;
        // float pwm_start=get_servo_pwm_value(PEN_UP_POS);
        // float gradual_pwm=pwm_start;
        // float pwm_step=(pwm_value-pwm_start)*0.5;
        // for(i=0;i<PEN_DOWN_STEPS;i++,gradual_pwm+=pwm_step,pwm_step=PEN_DOWN_SLOW_FACTOR*pwm_step)
        // {
        //   #ifdef DEBUG_SERVO
        //     char prompt[]="DOWN counter";
        //     debug_digit_info(prompt,sizeof(prompt),i,0);
        //     char info[]="DOWN pos:";
        //     debug_digit_info(info,sizeof(info),gradual_pwm,1);
	      //   #endif
        //   SERVO_OCR_REGISTER = (uint8_t)gradual_pwm; // Set PWM output level.  
        //   if(SERVO_OCR_REGISTER==pwm_value)          
        //     break;

        //   if(gradual_pwm==pwm_start)
        //     continue;
          
        //   delay_ms(50);
        //   pwm_start=gradual_pwm;
        // }

        //Ensure the final position。
        SERVO_OCR_REGISTER = pwm_value;
      }
      else
		    SERVO_OCR_REGISTER = pwm_value;
		}
	}
	
	uint8_t spindle_compute_pwm_value(float angle) 
	{
		return get_servo_pwm_value(angle);
//		SERVO_OCR_REGISTER = get_servo_pwm_value(angle);
//		return SERVO_OCR_REGISTER;
	}

#endif


// Immediately sets spindle running state with direction and spindle rpm via PWM, if enabled.
// Called by g-code parser spindle_sync(), parking retract and restore, g-code program end,
// sleep, and spindle stop override.
#ifdef VARIABLE_SPINDLE
  void spindle_set_state(uint8_t state, float rpm)
#else
  void _spindle_set_state(uint8_t state)
#endif
{
  if (sys.abort) { return; } // Block during abort.

  if (state == SPINDLE_DISABLE) { // Halt or set spindle direction and rpm.
  
    #ifdef VARIABLE_SPINDLE
      sys.spindle_speed = PEN_UP_POS;
    #endif
    spindle_stop();
  
  } else {
    
    #if !defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && !defined(ENABLE_DUAL_AXIS)
      if (state == SPINDLE_ENABLE_CW) {
        SPINDLE_DIRECTION_PORT &= ~(1<<SPINDLE_DIRECTION_BIT);
      } else {
        SPINDLE_DIRECTION_PORT |= (1<<SPINDLE_DIRECTION_BIT);
      }
    #endif
  
    #ifdef VARIABLE_SPINDLE
      spindle_set_speed(get_servo_pwm_value(rpm));
    #endif
    #if (defined(USE_SPINDLE_DIR_AS_ENABLE_PIN) && \
        !defined(SPINDLE_ENABLE_OFF_WITH_ZERO_SPEED)) || !defined(VARIABLE_SPINDLE)
      // NOTE: Without variable spindle, the enable bit should just turn on or off, regardless
      // if the spindle speed value is zero, as its ignored anyhow.
      #ifdef INVERT_SPINDLE_ENABLE_PIN
        SPINDLE_ENABLE_PORT &= ~(1<<SPINDLE_ENABLE_BIT);
      #else
        SPINDLE_ENABLE_PORT |= (1<<SPINDLE_ENABLE_BIT);
      #endif    
    #endif
  
  }
  
  sys.report_ovr_counter = 0; // Set to report change immediately
}


// G-code parser entry-point for setting spindle state. Forces a planner buffer sync and bails 
// if an abort or check-mode is active.
#ifdef VARIABLE_SPINDLE
  void spindle_sync(uint8_t state, float rpm)
  {
    if (sys.state == STATE_CHECK_MODE) { return; }
    protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
    spindle_set_state(state,rpm);
  }
#else
  void _spindle_sync(uint8_t state)
  {
    if (sys.state == STATE_CHECK_MODE) { return; }
    protocol_buffer_synchronize(); // Empty planner buffer to ensure spindle is set when programmed.
    _spindle_set_state(state);
  }
#endif

//自动笔控制函数, 提供给planner.c调用
void auto_pen_control(uint8_t is_rapid_move)
{
  static uint8_t current_pen_state = 0;
  uint8_t desired_pen_state = is_rapid_move ? 0 : 1;
  
  if (current_pen_state != desired_pen_state) {
    if (desired_pen_state) {
      // 落笔
      spindle_set_state(SPINDLE_ENABLE_CW, 110.0);
    } else {
      // 抬笔
      spindle_set_state(SPINDLE_DISABLE, 0.0);
    }
    current_pen_state = desired_pen_state;
  }
}
